#include <ros/ros.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <std_srvs/Trigger.h>
#include "nav_and_digging/BaseNav.h"
#include "tf/transform_datatypes.h"
#include <nav_msgs/Odometry.h>

#define ManipulationEnable 1


 
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
typedef actionlib::SimpleActionClient<std_srvs::Trigger> ManipulationClient;

ros::ServiceClient manipulation_client;
MoveBaseClient *ac_ptr;



void MoveOneStepAndDrillHole(move_base_msgs::MoveBaseGoal goal, 
                             ros::ServiceClient &manipulation_client, 
                             MoveBaseClient &ac){

  ROS_INFO("Sending goal");
  ac.sendGoal(goal);
 
  ac.waitForResult(); //ros::Duration(20.0)

  if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED){

    ROS_INFO("Hooray, the base moved successfuly");
    
    

#ifdef ManipulationEnable
    ROS_INFO("Manipulating...");
    
    //call manipulation_server
    std_srvs::Trigger srv;

    manipulation_client.call(srv.request, srv.response);
    manipulation_client.waitForExistence();
    // while(srv.responce.success == 0){};
    // manipulation_client.waitForServer();
    ROS_INFO("manipulation_server responce [success:%d]", srv.response.success);
    ROS_INFO("Manipulating Finished.");
#endif

  }
  else{
    ROS_INFO("The base failed to move for some reason");
  }

}

bool BaseNavServerCb(nav_and_digging::BaseNav::Request  &req,
         			       nav_and_digging::BaseNav::Response &res
                    ){
  // 显示请求数据
  ROS_INFO("BaseNav: delta_x:%f  delta_y:%f delta_theta:%f", req.delta_x, req.delta_y, req.delta_theta);

  
  //call move_base
  move_base_msgs::MoveBaseGoal NextGoal;
  NextGoal.target_pose.header.frame_id = "base_link";
  NextGoal.target_pose.header.stamp = ros::Time::now();
  NextGoal.target_pose.pose.position.x = req.delta_x;
  NextGoal.target_pose.pose.position.y = req.delta_y;

  geometry_msgs::Quaternion Q_msg =  tf::createQuaternionMsgFromYaw(-req.delta_theta * 3.1415926 / 180);
  NextGoal.target_pose.pose.orientation.x = Q_msg.x;
  NextGoal.target_pose.pose.orientation.y = Q_msg.y;
  NextGoal.target_pose.pose.orientation.z = Q_msg.z;
  NextGoal.target_pose.pose.orientation.w = Q_msg.w;

  MoveOneStepAndDrillHole(NextGoal, manipulation_client, *ac_ptr);

  // 设置反馈数据
	res.result = "Call BaseNavServer OK";
  return true;
}
 
int main(int argc, char** argv){
  
  ros::init(argc, argv, "nav_and_digging");

  ros::NodeHandle n;
  
  MoveBaseClient ac("move_base", true);
  manipulation_client = n.serviceClient<std_srvs::Trigger>("/manipulation_server");
  ros::ServiceServer nav_and_digging_server = n.advertiseService("/BaseNav_Server", BaseNavServerCb);

  ac_ptr = &ac;

  //wait for the action server to come up
  while(!ac.waitForServer(ros::Duration(5.0))){
    ROS_INFO("Waiting for the move_base action server to come up");
  }

#ifdef ManipulationEnable
  //wait for the MoveNextHole server to come up
  ros::service::waitForService("/manipulation_server");
#endif

  

  ROS_INFO("Service: \"/BaseNav_Server\"  come up, wait for calling");
  ROS_INFO("Ready to nav_and_digging.");
  ros::spin();

  return 0;
}
